#ifndef CORE_H_
#define CORE_H_

#include "precom.h"
#include "Model.h"
#include "3DSLoader.h"
#include "Path5C.h"
#include "SpaceCubeRep.h"
#include "HalfSphereGenerator.h"
#include "RootMinimizer.h"

class Core
{
private:
	SpaceCubeRep	m_space;
	Model			m_model;
	Path5C			m_path;
	
	unsigned long	m_indX, m_indY, m_indZ;

	GLuint			m_toolList;
	float			m_radius;
	float			m_step;
	float			m_bumpCoef;
	Path5CPoint		m_toolPos;
	fmath::vector3	m_toolAxis;

	std::list<fmath::vector3>	m_surfaceNormals;
	std::list<float>			m_distances;
	float						m_errorsMax, m_errorsMin, m_errorsMean;
	long						m_errorsNum;
	std::list<fmath::vector3>	m_stationaryPoints;
	std::list<fmath::vector3>	m_coords;
	std::list<const CubeElement*> m_triangleToCompute;
	std::list<const CubeElement*> m_triangleToCompute2;
	bool			m_showList1, m_showList2, m_showRays;

	HalfSphereGenerator m_sphereGenerator;
	RootMinimizer m_minimizer;

	bool CompareVectors(
		const fmath::vector3& a_cross1,
		const fmath::vector3& a_cross2,
		const fmath::vector3& a_cross3, 
		const fmath::vector3& a_negligibleDir) const;
	bool CompareVectors(
		const fmath::vector3& a_cross1,
		const fmath::vector3& a_cross2,
		const fmath::vector3& a_cross3) const;
protected:
	fmath::vector3 GetToolNormalForPoint(const float *a_point) const;
	void FindStationaryPoints();
	void MoveToolByMinimalReqDistance();
	void FillSpaceCubeRep();
	void DrawTriangleWithLines(unsigned long a_triangle, const Mesh* a_mesh) const;
	void DrawBumpTriangle(unsigned long a_triangle, const Mesh* a_mesh) const;
	void DrawRaysToCenter(unsigned long a_triangle, const Mesh* a_mesh) const;
	void DrawRaysFromTool(unsigned long a_triangle, const Mesh* a_mesh) const;
	void DrawStationaryDistance();
	bool FindTrianglesFacedToTool();
	bool IsFacedToTool(const CubeElement* a_triangle) const;
	bool IsFacedToTool2(const CubeElement* a_triangle) const;
	Box BoundTool() const;
	fmath::vector3 Cramer(const fmath::matrix33& a_A, const fmath::vector3& a_b) const;
	void CramerPrepareMatrix(const fmath::matrix33& a_A, const fmath::vector3& a_b, fmath::matrix33& o_A) const;
	fmath::vector3 Gauss(const fmath::matrix33& a_A, const fmath::vector3& a_b) const;
	fmath::vector3 Crout(const fmath::matrix33& a_A, const fmath::vector3& a_b) const;
	bool CroutLUFactorization(const fmath::matrix33& a_A, fmath::matrix33& o_L, fmath::matrix33& o_U) const;
	fmath::matrix33 GetLocalTangentSpace(const fmath::vector3& a_norm) const;
	void CalculateVectorOnTangentPlane(const fmath::vector3& a_point, const fmath::vector3& a_norm, fmath::vector3& o_vec) const;
	void AddTriangleToList(const CubeElement* a_elem, std::list<const CubeElement*>& o_list) const;
	void CountError(const fmath::vector3& a_p, const fmath::vector3& a_n );

	inline fmath::vector3 Crout_Lw_Eq_b(const fmath::matrix33& a_L, const fmath::vector3& a_b) const
	{
		fmath::vector3 w;
		w[0] = a_b[0] / a_L[0][0];
		w[1] = (a_b[1] - w[0] * a_L[0][1]) / a_L[1][1];
		w[2] = (a_b[2] - w[0] * a_L[0][2] - w[1]*a_L[1][2]) / a_L[2][2];
		return w;
	}
	inline fmath::vector3 Crout_Ux_Eq_w(const fmath::matrix33& a_U, const fmath::vector3& a_w) const
	{
		fmath::vector3 x;
		x[2] = a_w[2];
		x[1] = a_w[1] - a_U[2][1] * x[2];
		x[0] = a_w[0] - a_U[1][0] * x[1] - a_U[2][0] * x[2];
		return x;
	}
	// najpierw wokol osi oy a potem wokol oz
	inline fmath::vector3 GetToolAxisVector() const 
	{ 
		return fmath::vector3(
			cosf(fmath::DegToRad(m_toolPos.m_eulerAngles.y))*sinf(fmath::DegToRad(m_toolPos.m_eulerAngles.x)),
			sinf(fmath::DegToRad(m_toolPos.m_eulerAngles.y))*sinf(fmath::DegToRad(m_toolPos.m_eulerAngles.x)), 
			cosf(fmath::DegToRad(m_toolPos.m_eulerAngles.x))
			);
	}
public:
	Core();

	void Clear();
	void PrepareToFollowUp();
	void NextStep();
	void DrawCubeAndTriangles(bool a_raysToCenter);
	void CompileToolList(float a_radius);
	void DrawTool() const;
	bool CroutTest() const;

	//==================TESTOWE METODY======
	inline const std::list<fmath::vector3>& GetStacionaryList() const
	{
		return m_stationaryPoints;
	}
	inline const std::list<fmath::vector3>& GetCoordList() const
	{
		return m_coords;
	}
	inline unsigned int GetTriangle1Count() const
	{
		return static_cast<unsigned int>( m_triangleToCompute.size() );
	}
	inline unsigned int GetTriangle2Count() const
	{
		return static_cast<unsigned int>( m_triangleToCompute2.size() );
	}
	inline void ShowHideRays()
	{
		m_showRays = !m_showRays;
	}
	inline void ShowHideList1()
	{
		m_showList1 = !m_showList1;
	}
	inline void ShowHideList2()
	{
		m_showList2 = !m_showList2;
	}
	inline void MoveToolXF()
	{
		m_toolPos.m_point.x += m_step;
	}
	inline void MoveToolXB()
	{	
		m_toolPos.m_point.x -= m_step;
	}
	inline void MoveToolYF()
	{
		m_toolPos.m_point.y += m_step;
	}
	inline void MoveToolYB()
	{	
		m_toolPos.m_point.y -= m_step;
	}
	inline void MoveToolZF()
	{
		m_toolPos.m_point.z += m_step;
	}
	inline void MoveToolZB()
	{	
		m_toolPos.m_point.z -= m_step;
	}
	inline void RotateToolAlpha(float a_angle)
	{	
		m_toolPos.m_eulerAngles.x += a_angle;
		m_toolAxis = GetToolAxisVector();
	}
	inline void RotateToolBeta(float a_angle)
	{	
		m_toolPos.m_eulerAngles.y += a_angle;
		m_toolAxis = GetToolAxisVector();
	}
	inline void ClearLists() 
	{
		m_triangleToCompute.clear();
		m_triangleToCompute2.clear();
		m_space.ClearCollidingCubesList();
	}
	//=================================
	inline Box GetCurBoxTMP()
	{
		return GetCubeWhichOwnPoint(GetToolPosition());
	}
	inline const fmath::vector3& GetToolAxis() const
	{ 
		return m_toolAxis; 
	}
	inline const fmath::vector3& GetToolPosition() const
	{
		return m_toolPos.m_point;
	}
	inline Box GetCubeWhichOwnPoint(const fmath::vector3& a_point)
	{ 
		Box bbox;
		m_space.GetBoxForPoint(reinterpret_cast<const float*>(&(a_point.x)), bbox);
		return bbox;
	}
	inline int SavePath(wchar_t* a_fileName) 
	{
		return m_path.SaveToFile(a_fileName);
	}
	inline int LoadPath(wchar_t* a_fileName) 
	{ 
		return m_path.LoadFromFile(a_fileName); 
	}
	inline bool LoadModel(wchar_t* a_fileName) 
	{ 
		ClearLists();
		m_path.Clear();
		bool res = m_model.LoadModel(a_fileName);
		m_step = m_model.GetBBox().GetMaxSize() / 500.0f;
		//m_bumpCoef = m_model.GetBBox().GetMinSize() / 500.0f;
		m_toolPos.m_point = m_model.GetBBox().m_max;
		m_toolPos.m_eulerAngles.set(0.f, 0.f);
		return res;
	}
	inline GLuint GetCompiledToolList() const 
	{
		return m_toolList;
	}
	inline GLuint GetCompiledModelList() const
	{ 
		return m_model.GetCompiledModelList(); 
	}
	inline GLuint GetCompiledSpace() const
	{
		return m_space.GetCompiledSpace();
	}
	inline Box GetModelBBox() const
	{ 
		return m_model.GetBBox(); 
	}
	inline float GetTriangleSize() const
	{
		return m_space.GetTriangleSize();
	}
	inline void ChangeCube(unsigned long a_x, unsigned long a_y, unsigned long a_z)
	{
		m_indX += a_x; m_indY += a_y; m_indZ += a_z;
		m_space.CutIndexes(m_indX, m_indY, m_indZ);
	}
	inline void DrawIntersectingCubes() const
	{
		glBegin(GL_LINES);
		glColor3f(0.f, 1.f, 0.f);
		m_space.DrawCollidingCubes();
		glEnd();
	}
	inline float GetToolRadius()
	{
		return m_radius;
	}
	inline void SetToolRadius(float a_radius) 
	{
		m_radius = a_radius;
		CompileToolList(m_radius);
	}
	inline void SetToolPos(const fmath::vector3& a_pos)
	{
		m_toolPos.m_point = a_pos;
	}
	inline Path5CPoint GetToolData() const
	{
		return m_toolPos;
	}
	inline void ClearErrors()
	{
		m_errorsMin = m_errorsMax = m_errorsMean = 0.f;
		m_errorsNum = 0;
	}
	inline float GetMinError() const { return m_errorsMin; }
	inline float GetMaxError() const { return m_errorsMax; }
	inline float GetMeanError() const { return m_errorsMean/(float)m_errorsNum; }
};

#endif // CORE_H_
